﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;


namespace PrototypeZGame
{
    public class Dijskra
    {
        private static int findMinIndex(int[] w, bool[] r)
        {
            int min = int.MaxValue, minIndex = -1;
            for (int i = 0; i < w.Length; i++)
            {
                if (r[i] && w[i] < min)
                {
                    min = w[i];
                    minIndex = i;
                }
            }
            return minIndex;
        }

        private static List<int> getNeighbours(int f, int n, int X, int Y)
        {
            List<int> N = new List<int>();
            int x, y;
            x = f % X;
            y = f / X;

            if (x > 0) N.Add(f - 1);
            if (x < X - 1) N.Add(f + 1);
            if (y > 0) N.Add(f - X);
            if (y < Y - 1) N.Add(f + X);
            return N;
        }


        //public static int[,] getFieldsWeights(Terrain terrain, Vec2 S, Vec2 A, Vec2 B)
        //{
        //    Vec2 oldA = A, oldB = B, oldBA = new Vec2(B.x - A.x, B.y - A.y);

        //    // Szerokość i wysokość terenu
        //    Vec2 T = new Vec2(terrain.X, terrain.Y);

        //    // Korekcja, aby przeszukiwana przestrzeń nie wykraczała poza planszę
        //    if (A.x < 0) A.x = 0;
        //    if (A.y < 0) A.y = 0;
        //    if (B.x > T.x) B.x = T.x;
        //    if (B.y > T.y) B.y = T.y;

        //    // Szerokość i wysokość przeszukiwanej przestrzeni
        //    Vec2 BA = new Vec2(B.x - A.x, B.y - A.y);

        //    int s = TTI(S, A, BA);
            
        //    int n = BA.x * BA.y;
        //    bool[] r = new bool[n]; // Wierzchołek nie ma jeszcze najkrótszej ścieżki
        //    int[] w = new int[n]; // Waga wierzchołka

        //    for (int v = 0; v < n; v++)
        //    {
        //        r[v] = true;
        //        w[v] = int.MaxValue;
        //    }

        //    r[s] = false;
        //    w[s] = 0;

        //    foreach (int N in getNeighbours(s, n, BA.x, BA.y))
        //    {
        //        w[N] = terrain.moveCost(ITT(N, A, BA));
        //    }

        //    for (int i = 1; i < n; i++)
        //    {
        //        int u = findMinIndex(w, r);
        //        if (u >= 0)
        //        {
        //            r[u] = false;

        //            foreach (int v in getNeighbours(u, n, BA.x, BA.y))
        //            {
        //                if (r[v])
        //                {
        //                    int newFieldCost = w[u] + terrain.moveCost(ITT(v, A, BA));
                            
        //                    if (newFieldCost>0 && newFieldCost < w[v])
        //                    {
        //                        w[v] = newFieldCost;
        //                    }
        //                }
        //            }
        //        }
        //        else
        //        {
        //            break;
        //        }
        //    }


        //    int[,] ret = new int[oldBA.x, oldBA.y];
        //    Vec2 V;

        //    for (int x = 0; x < ret.GetLength(0); x++)
        //    {
        //        for (int y = 0; y < ret.GetLength(1); y++)
        //        {
        //            ret[x, y] = int.MaxValue;
        //        }
        //    }

        //    for (int v = 0; v < n; v++)
        //    {
        //        V = ITT(v, new Vec2(A.x-oldA.x,A.y-oldA.y), BA);
        //        ret[V.x, V.y] = w[v];
        //    }
        //    return ret;
        //}



        /// <summary>
        /// Współrzędne w terenie do indeksu w 1 - wymiarowej tablicy
        /// </summary>
        /// <param name="V">Współrzędne w terenie</param>
        /// <param name="A">Położenie lewego-górnego pola przeszukiwanej przestrzeni w terenie</param>
        /// <param name="BA">Wysokość i szerokość przeszukiwanej przestrzeni</param>
        /// <returns>Indeks w 1 wymiarowej tablicy</returns> 
        public static int TTI(Vec2 V, Vec2 A, Vec2 BA)
        {
            return (V.x-A.x) + (V.y - A.y) * BA.x;
        }

        private static Vec2 ITT(int v, Vec2 A, Vec2 BA)
        {
            return new Vec2(v % BA.x + A.x, v / BA.x + A.y);
        }

        public static List<Direction> getPath(Terrain terrain,Vec2 S, Vec2 D, Vec2 A, Vec2 B)
        {
            if (!terrain.isPositionValid(S) || !terrain.isPositionValid(D))
            {
                return null;
            }

            //List<Direction> path = getPathSimple(terrain,S,D);
            //if (path != null) return path;

            int[,] map = Dijskra.getFieldsWeights(terrain, S, A, B, D);
            return Dijskra.getPath(map, S - A, D - A);
        }


        public static List<Direction> getPathSimple(Terrain terrain, Vec2 S, Vec2 D)
        {
            int sum, min;
            Vec2 v, s, d;            
            int X = D.x - S.x, Y = D.y - S.y;
            bool east, south, turn = X == 0 && Y == 0;
            min = (X - 1) + (Y - 1);
            if (turn) min++;

            if (X > 0)
            {
                s.x = S.x;
                d.x = D.x;
                east = true;
            }
            else
            {
                s.x = D.x;
                d.x = S.x;
                east = false;
            }

            if (Y > 0)
            {
                s.y = S.y;
                d.y = D.y;
                south = true;
            }
            else
            {
                s.y = D.y;
                d.y = S.y;
                south = false;
            }


            // Horizontal then vertical
            sum = 0;
            v = s;
            for (v.x++; v.x < d.x; v.x++)
            {
                sum += terrain.moveCost(v);
            }
            if (turn)
            {
                sum += terrain.moveCost(v);
            }
            for (v.y++; v.y < d.y; v.y++)
            {
                sum += terrain.moveCost(v);
            }
            if (sum >= 0 && sum <= min)
            {
                List<Direction> path = new List<Direction>(sum + 1);

                Direction dir;
                if (east)
                {
                    dir = Direction.East;
                }
                else
                {
                    dir = Direction.West;
                }

                for (int x = 0; x < X; x++)
                {
                    path.Add(dir);
                }
                if (turn)
                {
                    path.Add(dir);
                }

                if (south)
                {
                    dir = Direction.South;
                }
                else
                {
                    dir = Direction.North;
                }

                for (int y = 0; y < Y; y++)
                {
                    path.Add(dir);
                }
                return path;
            }




            // vertical then horizontal
            sum = 0;
            v = s;
            for (v.y++; v.y < d.y; v.y++)
            {
                sum += terrain.moveCost(v);
            }
            if (turn)
            {
                sum += terrain.moveCost(v);
            }
            for (v.x++; v.x < d.x; v.x++)
            {
                sum += terrain.moveCost(v);
            }


            if (sum >= 0 && sum <= min)
            {
                List<Direction> path = new List<Direction>(sum + 1);

                Direction dir;

                if (south)
                {
                    dir = Direction.South;
                }
                else
                {
                    dir = Direction.North;
                }
                for (int y = 0; y < Y; y++)
                {
                    path.Add(dir);
                }

                if (turn)
                {
                    path.Add(dir);
                }

                if (east)
                {
                    dir = Direction.East;
                }
                else
                {
                    dir = Direction.West;
                }

                for (int x = 0; x < X; x++)
                {
                    path.Add(dir);
                }
                
                return path;
            }

            return null;
        }

        /// <summary>
        /// Constructs a path from results of Dijskra.getFieldsWeights
        /// </summary>
        /// <param name="map">Result of Dijskra.getFieldsWeights</param>
        /// <param name="S">"From" vector (in map coordinates)</param>
        /// <param name="D">"To" vector (in map coordinates)</param>
        /// <returns>Path from Source to Destination given as Directions to take</returns>
        public static List<Direction> getPath(int[,] map, Vec2 S, Vec2 D)
        {
            // Check if Destination is in map range
            if (D.x < 0 || D.x >= map.GetLength(0) || D.y < 0 || D.y >= map.GetLength(1)) return null;

            // Check if Source is in map range
            if (S.x < 0 || S.x >= map.GetLength(0) || S.y < 0 || S.y >= map.GetLength(1)) return null;

            // Check if Source is really a source
            if (map[S.x, S.y] != 0) return null;

            // Check if Destination is diffrent from Source
            if(S == D) return null;

            // Check if Destination is reachable
            if( map[D.x, D.y] == int.MaxValue ) return null;

            Vec2 v = D;
            List<Direction> path = new List<Direction>();
            int min;
            Direction next = new Direction();

            while (v != S)
            {
                // Choose next direction in a path
                min = map[v.x, v.y];
                if (v.y > 0 && map[v.x, v.y - 1] < min)
                {
                    min = map[v.x, --v.y];
                    next = Direction.South;
                } 
                if (v.y < map.GetLength(1)-1 && map[v.x, v.y + 1] < min)
                {
                    min = map[v.x, ++v.y];
                    next = Direction.North;
                } 
                if (v.x > 0 && map[v.x - 1, v.y] < min)
                { 
                    min = map[--v.x, v.y];
                    next = Direction.East;
                }
                if (v.x < map.GetLength(0)-1 && map[v.x + 1, v.y] < min)
                {
                    min = map[++v.x, v.y];
                    next = Direction.West;
                }
                path.Add(next);
            }

            // Path was made from destination to source, so reverse
            path.Reverse();
            return path;
        }

        public static int[,] getFieldsWeights(Terrain terrain, Vec2 S, Vec2 A, Vec2 B, Vec2 D)
        {
            Vec2 oldA = A, oldB = B, oldBA = new Vec2(B.x - A.x, B.y - A.y);

            // Szerokość i wysokość terenu
            Vec2 T = new Vec2(terrain.X, terrain.Y);

            // Korekcja, aby przeszukiwana przestrzeń nie wykraczała poza planszę
            if (A.x < 0) A.x = 0;
            if (A.y < 0) A.y = 0;
            if (B.x > T.x) B.x = T.x;
            if (B.y > T.y) B.y = T.y;

            // Szerokość i wysokość przeszukiwanej przestrzeni
            Vec2 BA = new Vec2(B.x - A.x, B.y - A.y);

            int s = TTI(S, A, BA);
            int d = TTI(D, A, BA);

            int n = BA.x * BA.y;
            bool[] r = new bool[n]; // Wierzchołek nie ma jeszcze najkrótszej ścieżki
            int[] w = new int[n]; // Waga wierzchołka

            for (int v = 0; v < n; v++)
            {
                r[v] = true;
                w[v] = int.MaxValue;
            }

            r[s] = false;
            w[s] = 0;

            foreach (int N in getNeighbours(s, n, BA.x, BA.y))
            {
                w[N] = terrain.moveCost(ITT(N, A, BA));
            }

            for (int i = 1; i < n; i++)
            {
                int u = findMinIndex(w, r);
                if (u >= 0)
                {
                    r[u] = false;

                    foreach (int v in getNeighbours(u, n, BA.x, BA.y))
                    {
                        if (r[v])
                        {
                            int newFieldCost = w[u] + terrain.moveCost(ITT(v, A, BA));

                            if (newFieldCost > 0 && newFieldCost < w[v])
                            {
                                w[v] = newFieldCost;
                            }
                        }
                    }
                    if (u == d)
                    {
                        break;
                    }
                }
                else
                {
                    break;
                }
            }


            int[,] ret = new int[oldBA.x, oldBA.y];
            Vec2 V;

            for (int x = 0; x < ret.GetLength(0); x++)
            {
                for (int y = 0; y < ret.GetLength(1); y++)
                {
                    ret[x, y] = int.MaxValue;
                }
            }

            for (int v = 0; v < n; v++)
            {
                V = ITT(v, new Vec2(A.x - oldA.x, A.y - oldA.y), BA);
                ret[V.x, V.y] = w[v];
            }
            return ret;
        }
    }



}
